Magnetometer bias and anomaly detector

ABSTRACT

The computer implemented method, system or computer program product comprises collecting magnetometer data from the device; and calculating a center of a shape of the magnetometer data as a result of minimization. The minimization of calculating the center of the shape further comprises calculating a plurality of running sums of the magnetometer data; storing the plurality of running sums; storing a count of the number of terms in each of the running sums; and calculating the center of the shape and setting the estimated magnetometer bias to the center of the shape. The radius of the sphere is calculated to ensure accuracy in the estimator of the magnetometer bias.

CROSS-REFERENCE TO RELATED APPLICATIONS

This application claims benefit under 35 USC 119(e) of ProvisionalApplication Ser. No. 61/524,703, filed on Aug. 17, 2011, entitled“MAGNETOMETER BIAS AND ANOMALY DETECTOR,” and is related to U.S.Provisional Patent Application No. 61/590,924, filed Jan. 26, 2012,entitled “IN-USE AUTOMATIC CALIBRATION METHODOLOGY FOR ACCELEROMETERS(AND/OR MAGNETOMETERS) IN MOBILE DEVICES,” all of which are incorporatedherein by reference in their entireties.

FIELD OF THE INVENTION

The present invention relates generally to devices that utilizemagnetometers and more specifically to detecting magnetometer bias insuch devices.

BACKGROUND OF THE INVENTION

Traditional methods of determining magnetometer bias do not meet all ofthe features of being robust to noise, closed form, low memory and fastconvergence. Accordingly what is needed is a system and method toaddress these features. The system and method should be adaptable,easily implemented and cost effective. The present invention addressessuch a need.

SUMMARY OF THE INVENTION

A computer implemented method, system or computer program product ofestimating magnetometer bias and axis sensitivity for a device isdisclosed. The computer implemented method, system or computer programproduct comprises collecting magnetometer data from the device; andcalculating a center of a shape of the magnetometer data as a result ofminimization. The minimization of calculating the center of the shapefurther comprises calculating a plurality of running sums of themagnetometer data; storing the plurality of running sums; storing acount of the number of terms in each of the running sums; andcalculating the center of the shape and setting the estimatedmagnetometer bias to the center of the shape.

The present invention provides a fast closed form method to determinemagnetometer bias which is robust with noise measurements. The presentinvention also provides methods to evaluate convergence of bias.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a block diagram of a system that utilizes a magnetometerdetection algorithm in accordance with an embodiment.

FIG. 2 is a flow chart which illustrates generally detectingmagnetometer bias in the device.

FIG. 3 is an example of a location of raw magnetometer data.

FIG. 4 is a flow chart for estimating a bias in accordance with anembodiment.

FIG. 5 is an example of computing the center point of a sphere.

DETAILED DESCRIPTION

The present invention relates generally to devices that utilizemagnetometers and more specifically to detecting magnetometer bias insuch devices. The following description is presented to enable one ofordinary skill in the art to make and use the invention and is providedin the context of a patent application and its requirements. Variousmodifications to the preferred embodiment and the generic principles andfeatures described herein will be readily apparent to those skilled inthe art. Thus, the present invention is not intended to be limited tothe embodiments shown, but is to be accorded the widest scope consistentwith the principles and features described herein.

To accurately determine magnetometer bias several assumptions must bemade. It is assumed that relative to the sensor the earth's magneticfield is constant and there is a small amount of magnetic disturbanceinterfering with the sensors. It is also assumed that the magnetometersensors sensitivity is equal on each of the three axes. With these threeassumptions, as a user moves a magnetometer sensor the shape of themeasurements will start to appear spherical. From a geometric standpointthe center of the sphere is then found, the center of the sphererepresents the magnetometer bias. The present invention is directed todetermining the magnetometer bias to ensure the device is in its properorientation. To describe the features of the present invention in moredetail refer now to the following description in conjunction with theaccompanying figures.

FIG. 1 is a block diagram of a system 10 that utilizes a magnetometerestimator algorithm 100 in accordance with an embodiment. The system 10includes a device 11. The device 11 may include any type of mobiledevice including but not limited to, a cell phone, a tablet PC, or othertype of portable electronic device. The device 11 receives input datafrom a magnetometer 12. The device 11 includes memory 18 for storingdata. Data 20 stores data from the magnetometer 12. Memory 18 alsoincludes a bias detector algorithm 100 in accordance with the presentinvention. A processor 24 executes the algorithm 100 which operates onthe data received from magnetometer 12. The processor 24 provides theexecuted data to sensor fusion system 16. The sensor fusion system 16provides orientation information of the device.

FIG. 2 is a flow chart which illustrates generally detectingmagnetometer bias in the device. As is seen, first magnetometer data iscollected, via step 102. Next, the center of an object is calculated asa result of minimization, via step 104. The center of the object is thenstored in the memory, via step 106. The object could be a sphere orellipsoid depending on the device movement.

A system and method in accordance with the present invention estimatesthe bias for a magnetometer sensor with three degrees of freedom. Asystem and method in accordance with the present invention is utilizedto recognize magnetic disturbances both before and after a bias hasconverged.

A system that utilizes a magnetometer bias estimation algorithm inaccordance with the present invention can take the form of an entirelyhardware implementation, an entirely software implementation, or animplementation containing both hardware and software elements. In oneimplementation, the magnetometer bias estimation algorithm isimplemented in software, which includes, but is not limited to,application software, firmware, resident software, microcode, etc.

Furthermore, the magnetometer bias estimation procedure can take theform of a computer program product accessible from a computer-usable orcomputer-readable medium providing program code for use by or inconnection with a computer or any instruction execution system. For thepurposes of this description, a computer-usable or computer-readablemedium can be any apparatus that can contain, store, communicate,propagate, or transport the program for use by or in connection with theinstruction execution system, apparatus, or device.

The medium can be an electronic, magnetic, optical, electromagnetic,infrared, or semiconductor system (or apparatus or device) or apropagation medium. Examples of a computer-readable medium include asemiconductor or solid state memory, magnetic tape, a removable computerdiskette, a random access memory (RAM), a read-only memory (ROM), arigid magnetic disk, and an optical disk. Current examples of opticaldisks include DVD, compact disk-read-only memory (CD-ROM), and compactdisk-read/write (CD-R/W). To describe the features of the presentinvention in more detail, refer now to the following description inconjunction with the accompanying figures.

FIG. 3 is an example of raw magnetometer data. FIG. 3 illustrates thatas magnetometer data is being gathered, a spherical shape of the data isstarting to form. What is desired is, which has been described before,is to determine the center of an object such as a sphere so that themagnetometer bias can be determined. To describe determining the centerof the sphere of the magnetometer data refer now to the following.

Center of the Sphere

As shown in FIG. 5, given a pair of points, X and Y, it is possible tocompute the dot product of the vector from the midpoint of the pair ofpoints to the center of the sphere (Vector 1), and the normalized vectorbetween the two points (Vector 2). If there is no error and both points(X and Y) lie on the sphere with Point C being the center of the sphere,the dot product value will be zero. Another interpretation of the dotproduct value is it is the distance between the center of the sphere andthe plane created by the midpoint of the two points and the normalvector created by the two points. The following S′ equation representssquaring the dot product for all combinations of points:

$\begin{matrix}{S^{\prime} = {\sum\limits_{n = 1}^{N - 1}{\sum\limits_{m = {n + 1}}^{N}\frac{\begin{bmatrix}{{\left\lbrack {a - \frac{\left( {x_{m} + x_{n}} \right)}{2}} \right\rbrack \cdot \left( {x_{m} - x_{n}} \right)} + {\left\lbrack {b - \frac{\left( {y_{m} + y_{n}} \right)}{2}} \right\rbrack \cdot}} \\{\left( {y_{m} - y_{n}} \right) + {\left\lbrack {c - \frac{\left( {z_{m} + z_{n}} \right)}{2}} \right\rbrack \cdot \left( {z_{m} - z_{n}} \right)}}\end{bmatrix}^{2}}{\left( {x_{m} - x_{n}} \right)^{2} + \left( {y_{m} - y_{n}} \right)^{2} + \left( {z_{m} - z_{n}} \right)^{2}}}}} & (1)\end{matrix}$where a, b, c represents the center of the sphere, and x, y, zrepresents the magnetometer measurements. There are N number ofmagnetometer measurements.

Next, because the data points have errors on them, the equation will beweighted by how far apart the pairs of points are. The result will give:

$\begin{matrix}{S = {\sum\limits_{n = 1}^{N - 1}{\sum\limits_{m = {n + 1}}^{N}\begin{bmatrix}{{\left\lbrack {a - \frac{\left( {x_{m} + x_{n}} \right)}{2}} \right\rbrack \cdot \left( {x_{m} - x_{n}} \right)} + {\left\lbrack {b - \frac{\left( {y_{m} + y_{n}} \right)}{2}} \right\rbrack \cdot}} \\{\left( {y_{m} - y_{n}} \right) + {\left\lbrack {c - \frac{\left( {z_{m} + z_{n}} \right)}{2}} \right\rbrack \cdot \left( {z_{m} - z_{n}} \right)}}\end{bmatrix}^{2}}}} & (2)\end{matrix}$

The Equation (2) can be minimized by taking the partial derivatives andsetting the partial derivatives to zero. Taking partial derivatives of Swith respect to a, b, c and setting it to zero gives the set ofequations (3):

$\left. {(3)\begin{matrix}{{\sum\limits_{n = 1}^{N - 1}{\sum\limits_{m = {n + 1}}^{N}\begin{bmatrix}{{- 2} \cdot \left( {x_{m} - x_{n}} \right) \cdot \left\lbrack {{\left( {x_{m} - x_{n}} \right) \cdot \left( {\frac{x_{m}}{2} - a + \frac{x_{n}}{2}} \right)} + {\left( {y_{m} - y_{n}} \right) \cdot}} \right.} \\\left. {\left( {\frac{y_{m}}{2} - b + \frac{y_{n}}{2}} \right) + {\left( {z_{m} - z_{n}} \right) \cdot \left( {\frac{z_{m}}{2} - c + \frac{z_{n}}{2}} \right)}} \right\rbrack\end{bmatrix}}} = 0} \\{{\sum\limits_{n = 1}^{N - 1}{\sum\limits_{m = {n + 1}}^{N}\begin{bmatrix}{{- 2} \cdot \left( {y_{m} - y_{n}} \right) \cdot \left\lbrack {{\left( {x_{m} - x_{n}} \right) \cdot \left( {\frac{x_{m}}{2} - a + \frac{x_{n}}{2}} \right)} + {\left( {y_{m} - y_{n}} \right) \cdot}} \right.} \\\left. {\left( {\frac{y_{m}}{2} - b + \frac{y_{n}}{2}} \right) + {\left( {z_{m} - z_{n}} \right) \cdot \left( {\frac{z_{m}}{2} - c + \frac{z_{n}}{2}} \right)}} \right\rbrack\end{bmatrix}}} = 0} \\{{\sum\limits_{n = 1}^{N - 1}{\sum\limits_{m = {n + 1}}^{N}\begin{bmatrix}{{- 2} \cdot \left( {z_{m} - z_{n}} \right) \cdot \left\lbrack {{\left( {x_{m} - x_{n}} \right) \cdot \left( {\frac{x_{m}}{2} - a + \frac{x_{n}}{2}} \right)} + {\left( {y_{m} - y_{n}} \right) \cdot}} \right.} \\\left. {\left( {\frac{y_{m}}{2} - b + \frac{y_{n}}{2}} \right) + {\left( {z_{m} - z_{n}} \right) \cdot \left( {\frac{z_{m}}{2} - c + \frac{z_{n}}{2}} \right)}} \right\rbrack\end{bmatrix}}} = 0}\end{matrix}} \right\}$

To reduce the double sums to single sums the following identity is used:

$\begin{matrix}{{\sum\limits_{i = 1}^{N - 1}{\sum\limits_{j = {i + 1}}^{N}\left\lbrack {\left( {b_{j} - b_{i}} \right) \cdot \left( {a_{j} - a_{i}} \right)} \right\rbrack}} = {\left\lbrack {N \cdot {\sum\limits_{i = 1}^{N}\left( {b_{i} \cdot a_{i}} \right)}} \right\rbrack - {\left\lbrack {\sum\limits_{i = 1}^{N}\left( b_{i} \right)} \right\rbrack \cdot \left\lbrack {\sum\limits_{i = 1}^{N}\left( a_{i} \right)} \right\rbrack}}} & (4)\end{matrix}$

The following terms result:

$\begin{matrix}{\mspace{79mu}{{{Am} = {{N \cdot {\sum\limits_{n = 1}^{N}\left( x_{n} \right)^{2}}} - \left( {\sum\limits_{n = 1}^{N}x_{n}} \right)^{2}}}\mspace{79mu}{{Bm} = {{N \cdot {\sum\limits_{n = 1}^{N}\left( y_{n} \right)^{2}}} - \left( {\sum\limits_{n = 1}^{N}y_{n}} \right)^{2}}}\mspace{79mu}{{Cm} = {{N \cdot {\sum\limits_{n = 1}^{N}\left( z_{n} \right)^{2}}} - \left( {\sum\limits_{n = 1}^{N}z_{n}} \right)^{2}}}\mspace{79mu}{{Dm} = {{N \cdot {\sum\limits_{n = 1}^{N}\left( {x_{n} \cdot y_{n}} \right)}} - {\sum\limits_{n = 1}^{N}{x_{n} \cdot {\sum\limits_{n = 1}^{N}y_{n}}}}}}\mspace{79mu}{{Em} = {{N \cdot {\sum\limits_{n = 1}^{N}\left( {x_{n} \cdot z_{n}} \right)}} - {\sum\limits_{n = 1}^{N}{x_{n} \cdot {\sum\limits_{n = 1}^{N}z_{n}}}}}}\mspace{79mu}{{Fm} = {{N \cdot {\sum\limits_{n = 1}^{N}\left( {y_{n} \cdot z_{n}} \right)}} - {\sum\limits_{n = 1}^{N}{y_{n} \cdot {\sum\limits_{n = 1}^{N}z_{n}}}}}}{{Gm} = {\frac{1}{2} \cdot {\quad{\sum\limits_{n = 1}^{N - 1}{\sum\limits_{m = {n + 1}}^{N}\left\lbrack {{{\left( {x_{m} - x_{n}} \right) \cdot \left. \quad\left\lbrack {\left( x_{m} \right)^{2} - \left( x_{n} \right)^{2} + \left( y_{m} \right)^{2} - \left( y_{n} \right)^{2} + \left( z_{m} \right)^{2} - \left( z_{n} \right)^{2}} \right\rbrack \right\rbrack}{Hm}} = {\frac{1}{2} \cdot {\quad{\sum\limits_{n = 1}^{N - 1}{\sum\limits_{m = {n + 1}}^{N}\left\lbrack {{{\left( {y_{m} - y_{n}} \right) \cdot \left. \quad\left\lbrack {\left( x_{m} \right)^{2} - \left( x_{n} \right)^{2} + \left( y_{m} \right)^{2} - \left( y_{n} \right)^{2} + \left( z_{m} \right)^{2} - \left( z_{n} \right)^{2}} \right\rbrack \right\rbrack}{Im}} = {\frac{1}{2} \cdot {\quad{\sum\limits_{n = 1}^{N - 1}{\sum\limits_{m = {n + 1}}^{N}\left\lbrack {\left( {z_{m} - z_{n}} \right) \cdot {\quad\left\lbrack {\left( x_{m} \right)^{2} - \left( x_{n} \right)^{2} + \left( y_{m} \right)^{2} - \left( y_{n} \right)^{2} + \left( z_{m} \right)^{2} - \left( z_{n} \right)^{2}} \right\rbrack}} \right\rbrack}}}}} \right.}}}}} \right.}}}}}}} & (5)\end{matrix}$

Which is equivalent to:

$\begin{matrix}\left. \begin{matrix}\begin{matrix}{{Gm} = {\frac{1}{2} \cdot {\sum\limits_{n = 1}^{N - 1}{\sum\limits_{m = {n + 1}}^{N}\begin{bmatrix}{{\left( {x_{m} - x_{n}} \right) \cdot \left\lbrack {\left( x_{m} \right)^{2} - \left( x_{n} \right)^{2}} \right\rbrack} + {\left( {x_{m} - x_{n}} \right) \cdot}} \\{\left\lbrack {\left( y_{m} \right)^{2} - \left( y_{n} \right)^{2}} \right\rbrack + {\left( {x_{m} - x_{n}} \right) \cdot \left\lbrack {\left( z_{m} \right)^{2} - \left( z_{n} \right)^{2}} \right\rbrack}}\end{bmatrix}}}}} \\{{Hm} = {\frac{1}{2} \cdot {\sum\limits_{n = 1}^{N - 1}{\sum\limits_{m = {n + 1}}^{N}\begin{bmatrix}{{\left( {y_{m} - y_{n}} \right) \cdot \left\lbrack {\left( x_{m} \right)^{2} - \left( x_{n} \right)^{2}} \right\rbrack} + {\left( {y_{m} - y_{n}} \right) \cdot}} \\{\left\lbrack {\left( y_{m} \right)^{2} - \left( y_{n} \right)^{2}} \right\rbrack + {\left( {y_{m} - y_{n}} \right) \cdot \left\lbrack {\left( z_{m} \right)^{2} - \left( z_{n} \right)^{2}} \right\rbrack}}\end{bmatrix}}}}}\end{matrix} \\{{Im} = {\frac{1}{2} \cdot {\sum\limits_{n = 1}^{N - 1}{\sum\limits_{m = {n + 1}}^{N}\begin{bmatrix}{{\left( {z_{m} - z_{n}} \right) \cdot \left\lbrack {\left( x_{m} \right)^{2} - \left( x_{n} \right)^{2}} \right\rbrack} + {\left( {z_{m} - z_{n}} \right) \cdot}} \\{\left\lbrack {\left( y_{m} \right)^{2} - \left( y_{n} \right)^{2}} \right\rbrack + {\left( {z_{m} - z_{n}} \right) \cdot \left\lbrack {\left( z_{m} \right)^{2} - \left( z_{n} \right)^{2}} \right\rbrack}}\end{bmatrix}}}}}\end{matrix} \right\} & (6)\end{matrix}$

Reducing to single sums as

$\begin{matrix}\left. \begin{matrix}\begin{matrix}{{Gm} = {\frac{1}{2} \cdot \begin{bmatrix}{{N \cdot {\sum\limits_{n = 1}^{N}\left( x_{n} \right)^{3}}} + {N \cdot {\sum\limits_{n = 1}^{N}\left\lbrack {x_{n} \cdot \left( y_{n} \right)^{2}} \right\rbrack}} + {N \cdot}} \\{{\sum\limits_{n = 1}^{N}\left\lbrack {x_{n} \cdot \left( z_{n} \right)^{2}} \right\rbrack} - {\sum\limits_{n = 1}^{N}{x_{n} \cdot \begin{bmatrix}{{\sum\limits_{n = 1}^{N}\left( x_{n} \right)^{2}} +} \\{{\sum\limits_{n = 1}^{N}\left( y_{n} \right)^{2}} + {\sum\limits_{n = 1}^{N}\left( z_{n} \right)^{2}}}\end{bmatrix}}}}\end{bmatrix}}} \\{{Hm} = {\frac{1}{2} \cdot \begin{bmatrix}{{N \cdot {\sum\limits_{n = 1}^{N}\left\lbrack {y_{n} \cdot \left( x_{n} \right)^{2}} \right\rbrack}} + {N \cdot {\sum\limits_{n = 1}^{N}\left( y_{n} \right)^{3}}} + {N \cdot}} \\{{\sum\limits_{n = 1}^{N}\left\lbrack {y_{n} \cdot \left( z_{n} \right)^{2}} \right\rbrack} - {\sum\limits_{n = 1}^{N}{y_{n} \cdot \begin{bmatrix}{{\sum\limits_{n = 1}^{N}\left( x_{n} \right)^{2}} +} \\{{\sum\limits_{n = 1}^{N}\left( y_{n} \right)^{2}} + {\sum\limits_{n = 1}^{N}\left( z_{n} \right)^{2}}}\end{bmatrix}}}}\end{bmatrix}}}\end{matrix} \\{{Im} = {\frac{1}{2} \cdot \begin{bmatrix}{{N \cdot {\sum\limits_{n = 1}^{N}\left\lbrack {z_{n} \cdot \left( x_{n} \right)^{2}} \right\rbrack}} + {N \cdot {\sum\limits_{n = 1}^{N}\left\lbrack {z_{n} \cdot \left( y_{n} \right)^{2}} \right\rbrack}} + {N \cdot}} \\{{\sum\limits_{n = 1}^{N}\left( z_{n} \right)^{3}} - {\sum\limits_{n = 1}^{N}{z_{n} \cdot \begin{bmatrix}{{\sum\limits_{n = 1}^{N}\left( x_{n} \right)^{2}} +} \\{{\sum\limits_{n = 1}^{N}\left( y_{n} \right)^{2}} + {\sum\limits_{n = 1}^{N}\left( z_{n} \right)^{2}}}\end{bmatrix}}}}\end{bmatrix}}}\end{matrix} \right\} & (7)\end{matrix}$

To solve the three system of equations, they can be rewritten in matrixform as:

$\begin{matrix}{{{\begin{pmatrix}{Am} & {Dm} & {Em} \\{Dm} & {Bm} & {Fm} \\{Em} & {Fm} & {Cm}\end{pmatrix} \cdot \begin{pmatrix}a \\b \\c\end{pmatrix}} = \begin{pmatrix}{Gm} \\{Hm} \\{Im}\end{pmatrix}}{\begin{pmatrix}a \\b \\c\end{pmatrix} = \begin{bmatrix}\frac{\begin{matrix}{{{Em} \cdot \left( {{{Bm} \cdot {Im}} - {{Fm} \cdot {Hm}}} \right)} + {{Fm}^{2} \cdot {Gm}} - {{Bm} \cdot}} \\{{{Cm} \cdot {Gm}} + {{Cm} \cdot {Dm} \cdot {Hm}} - {{Dm} \cdot {Fm} \cdot {Im}}}\end{matrix}}{{{Cm} \cdot {Dm}^{2}} - {2 \cdot {Dm} \cdot {Em} \cdot {Fm}} + {{Bm} \cdot {Em}^{2}} +} \\{{{Am} \cdot {Fm}^{2}} - {{Am} \cdot {Bm} \cdot {Cm}}} \\\frac{\begin{matrix}{{{Fm} \cdot \left( {{{Am} \cdot {Im}} - {{Em} \cdot {Gm}}} \right)} + {{Em}^{2} \cdot {Hm}} - {{Am} \cdot}} \\{{{Cm} \cdot {Hm}} + {{Cm} \cdot {Dm} \cdot {Gm}} - {{Dm} \cdot {Em} \cdot {Im}}}\end{matrix}}{\begin{matrix}{{{Cm} \cdot {Dm}^{2}} - {2 \cdot {Dm} \cdot {Em} \cdot {Fm}} + {{Bm} \cdot {Em}^{2}} +} \\{{{Am} \cdot {Fm}^{2}} - {{Am} \cdot {Bm} \cdot {Cm}}} \\\frac{\begin{matrix}{{{Fm} \cdot \left( {{{Am} \cdot {Hm}} - {{Dm} \cdot {Gm}}} \right)} + {{Dm}^{2} \cdot {Im}} - {{Am} \cdot}} \\{{{Bm} \cdot {Im}} + {{Bm} \cdot {Em} \cdot {Gm}} - {{Dm} \cdot {Em} \cdot {Hm}}}\end{matrix}}{\begin{matrix}{{{Cm} \cdot {Dm}^{2}} - {2 \cdot {Dm} \cdot {Em} \cdot {Fm}} + {{Bm} \cdot {Em}^{2}} +} \\{{{Am} \cdot {Fm}^{2}} - {{Am} \cdot {Bm} \cdot {Cm}}}\end{matrix}}\end{matrix}}\end{bmatrix}}} & (8)\end{matrix}$

One advantage of the above equations (8) is that while maintaining usageof all the historical data, only the running summations need to bestored. Since only the running summations need to be stored memoryresources utilized are reduced which is always an advantage on anembedded resource.

To maintain numerical stability and to give more weight to newer data tohandle cases of the magnetometer bias slowly changing, the summationparameters and the number of terms can all be divided by a constant.Another method to add numerical stability is to only add data tosections that have observed fewer orientations. Running sums arecomputed

$\begin{matrix}{{Q_{x} = {\left( {P_{x,{n - 1}} + \frac{\left( {x_{n} - a} \right)}{\sqrt{\left( {x_{n} - a} \right)^{2} + \left( {y_{n} - b} \right)^{2} + \left( {z_{n} - c} \right)^{2}}}} \right)*\left( \frac{n - 1}{n} \right)}}{Q_{y} = {\left( {P_{y,{n - 1}} + \frac{\left( {y_{n} - a} \right)}{\sqrt{\left( {x_{n} - a} \right)^{2} + \left( {y_{n} - b} \right)^{2} + \left( {z_{n} - c} \right)^{2}}}} \right)*\left( \frac{n - 1}{n} \right)}}{Q_{z} = {\left( {P_{z,{n - 1}} + \frac{\left( {z_{n} - a} \right)}{\sqrt{\left( {x_{n} - a} \right)^{2} + \left( {y_{n} - b} \right)^{2} + \left( {z_{n} - c} \right)^{2}}}} \right)*\left( \frac{n - 1}{n} \right)}}} & {{Equation}\mspace{14mu} 8a}\end{matrix}$With initialization shown in Equation 8b for the first sample

$\begin{matrix}{{P_{x,1} = \frac{\left( {x_{1} - a} \right)}{\sqrt{\left( {x_{1} - a} \right)^{2} + \left( {y_{1} - b} \right)^{2} + \left( {z_{1} - c} \right)^{2}}}}{P_{y,1} = \frac{\left( {y_{1} - a} \right)}{\sqrt{\left( {x_{1} - a} \right)^{2} + \left( {y_{1} - b} \right)^{2} + \left( {z_{1} - c} \right)^{2}}}}{P_{z,1} = \frac{\left( {z_{1} - a} \right)}{\sqrt{\left( {x_{1} - a} \right)^{2} + \left( {y_{1} - b} \right)^{2} + \left( {z_{1} - c} \right)^{2}}}}} & {{Equation}\mspace{14mu} 8b}\end{matrix}$Another way to interpret Equation 8a is the normalized average of sensordata. If the absolute value of Q terms in Equation 8a are below athreshold, then the compass sample (x_(n), y_(n), z_(n)) will be addedto the overall solution in equations 5, 6, 7 and 8. One implementationof the logic is shown below in Equation 8c. Other deviations fromEquation 8c that would work would be to allow 1 or more of thecomparisons to pass the threshold and to use multiple thresholds. Forinstance, an acceptable method would be to have a lower threshold for atleast 1 comparison to be true and a higher threshold all 3 comparisonsto be true before allowing the update to equations 5, 6, 7, 8.If ∥Q_(x)∥<T and ∥Q_(y)∥<T and ∥Q_(z)∥<T thenP_(x,n)=Q_(x)P_(y,n)=Q_(y)P_(s,n)=Q_(z)  Equation 8cUpdate to equations 5, 6, 7, 8

FIG. 4 is a flow chart for estimating a bias in accordance with anembodiment. First, magnetometer data is collected, via step 202. Thecenter of a shape of the magnetometer data is then calculated as aresult of minimization. To provide the minimization of calculating thecenter of the shape further includes the steps described below. First aplurality of running sums of the magnetometer data are calculated, viastep 204. In the above embodiment, the plurality of running sumscomprise running sums of the magnetometer data, the square of themagnetometer data, the cube of the magnetometer data and the crossproduct of the magnetometer data. Thereafter, the plurality of runningsums is stored in memory, via step 206. A count of the number of timesthat the plurality of running sums calculated is stored, via step 208.Then, the estimated magnetometer bias is calculated from the storedrunning sums when the count is less than a predetermined threshold, viastep 210. Finally, a center of the object is determined from theestimated magnetometer bias, via step 212.

Radius of Sphere

It is also desirable to compute the radius of the sphere to ensureaccuracy in the estimator of the bias. There are several methods tocompute the radius of the sphere. If the error between the radius andthe distance to the center is minimized, the results are as follows.

$\begin{matrix}{P = {\sum\limits_{n = 1}^{N}\left\lbrack {R - \sqrt{\left( {a - x_{n}} \right)^{2} + \left( {b - y_{n}} \right)^{2} + \left( {c - z_{n}} \right)^{2}}} \right\rbrack^{2}}} & (9) \\{\frac{\mathbb{d}P}{\mathbb{d}R} = {{2 \cdot N \cdot R} - {2 \cdot {\sum\limits_{n = 1}^{N}\sqrt{\left( {a - x_{n}} \right)^{2} + \left( {b - y_{n}} \right)^{2} + \left( {c - z_{n}} \right)^{2}}}}}} & (10) \\{R = {\frac{1}{N} \cdot {\sum\limits_{n = 1}^{N}\sqrt{\left( {a - x_{n}} \right)^{2} + \left( {b - y_{n}} \right)^{2} + \left( {c - z_{n}} \right)^{2}}}}} & (11)\end{matrix}$

The above equation has issues in that if the center (a, b, c) changes,then the set of data to recompute the radius would have to be stored.Another method would be to take the average of the square distance forthe radius squared

$\begin{matrix}{R^{2} = {\frac{1}{N} \cdot {\sum\limits_{n = 1}^{N}\left\lbrack {\left( {x_{n} - a} \right)^{2} + \left( {y_{n} - b} \right)^{2} + \left( {z_{n} - c} \right)^{2}} \right\rbrack}}} & (12)\end{matrix}$

which allows keeping the running sums as described above when the centerof the sphere was computed:

$\begin{matrix}{R^{2} = {\frac{1}{N} \cdot \left\lbrack {{\sum\limits_{n = 1}^{N}\left\lbrack \left( x_{n} \right)^{2} \right\rbrack} - {2 \cdot a \cdot {\sum\limits_{n = 1}^{N}x_{n}}} + {N \cdot a^{2}} + {\sum\limits_{n = 1}^{N}\left\lbrack \left( y_{n} \right)^{2} \right\rbrack} - {2 \cdot b \cdot {\sum\limits_{n = 1}^{N}y_{n}}} + {N \cdot b^{2}} + {\sum\limits_{n = 1}^{N}\left\lbrack \left( x_{n} \right)^{2} \right\rbrack} - {2 \cdot c \cdot {\sum\limits_{n = 1}^{N}z_{n}}} + {N \cdot c^{2}}} \right\rbrack}} & (13)\end{matrix}$Given the radius squared, we simply take the square root to obtain theradius.Evaluating Results

It must be determined how well the measurements can be trusted and ifthe measurements have been corrupted with a magnetic disturbance.Because the user may move the sensors near extraneous magnetic fields,the magnetometer measurements cannot be relied on. If the magnetometermeasurements cannot be trusted, then the magnetometer bias cannot becomputed. In the next section, several metrics will be described thatcan be used together to determine how accurate the bias computation is.Any combination of the listed metrics could be used.

Minimizing Term

The basis for finding the magnetometer bias was to minimize the S termabove and below. If the measured magnetometer data is not veryspherical, the minimization will be larger. To find out how well theestimate has measured the center of the circle, the value can becomputed which it originally tried to minimize. The equations can bereduced to single sums as was the case when computing the partialderivatives. Applying the double sum to single sum identity twenty-onetimes results in a single sum equation to compute S. Matlab code cantocompute S is shown below. The Matlab code takes the magnetometer data(x, y, z) and the bias or center of the spherical fit (a, b, c), thenreturns S, the term that was minimized. The running sums are done at thebeginning of the function, then S is computed. The Matlab program is forillustrative purposes only and could be implemented in a manner thatwould use running sums and not need a vector of data (x, y, z).

$\begin{matrix}{S = {\sum\limits_{n = 1}^{N - 1}{\sum\limits_{m = {n + 1}}^{N}\begin{bmatrix}{{\left\lbrack {a - \frac{\left( {x_{m} + x_{n}} \right)}{2}} \right\rbrack \cdot \left( {x_{m} - x_{n}} \right)} + {\left\lbrack {b - \frac{\left( {y_{m} + y_{n}} \right)}{2}} \right\rbrack \cdot}} \\{\left( {y_{m} - y_{n}} \right) + {\left\lbrack {c - \frac{\left( {z_{m} + z_{n}} \right)}{2}} \right\rbrack \cdot \left( {z_{m} - z_{n}} \right)}}\end{bmatrix}^{2}}}} & (14)\end{matrix}$

-   function s=centersum (x, y, z, a, b, c)-   % a, b, c is the center of the sphere-   % x, y, z 3D points.-   % Computes the sum of the distance squared between the center point    (a, b, c)-   % and a plane created by a normal vector between pairs of points and    the-   % midpoint of the pairs of points.    N=length(x);    x1=sum(x);    x2=sum(x.*x);    x3=sum(x.*x.*x);    x4=sum(x.*x.*x.*x);    y1=sum(y);    y2=sum(y.*y);    y3=sum(y.*y.*y);    y4=sum(y.*y.*y.*y);    z1=sum(z);    z2=sum(z.*z);    z3=sum(z.*z.*z);    z4=sum(z.*z.*z.*z);    x1y1=sum(x.*y);    x1z1=sum(x.*z);    y1z1=sum(y.*z);    x1y2=sum(x.*y.*y);    x1z2=sum(x.*z.*z);    y1z2=sum(y.*z.*z);    x2y1=sum(x.*x.*y);    x2z1=sum(x.*x.*z);    y2z1=sum(y.*y.*z);    x2y2=sum(x.*x.*y.*y);    x2z2=sum(x.*x.*z.*z);    y2z2=sum(y.*y.*z.*z);    s=N*a^2*x2−a^2*x1^2;    s=s−N*a*x3+a*x2*x1;    s=s+2*N*a*b*x1y1−2*a*b*x1*y1;    s=s−N*a*x1y2+a*x1*y2;    s=s+2*N*a*c*x1z1−2*a*c*x1*z1;    s 32 s−N*a*x1z2+a*x1*z2;    s=s+0.25*N*x4−0.25*x2*x2;    s=s−N*b*x2y1+b*x2*y1;    s=s+0.5*N*x2y2−0.5*x2*y2;    s=s−N*c*x2z1+c*x2*z1;    s=s+0.5*N*x2z2−0.5*x2*z2;    s=s+N*b^2*y2−b^2*y1^2;    s=s−N*b*y3+b*y2*y1;    s=s+2*N*b*c*y1z1−2*b*c*y1*z1;    s=s−N*b*y1z2+b*y1*z2;    s=s+0.25*N*y4−0.25*y2*y2;    s=s−N*c*y2z1+c*y2*z1;    s=s+0.5*N*y2z2−0.5*y2*z2;    s=s+N*c^2*z2−c^2*z1^2;    s=s−N*c*z3 +c*z2*z1;    s=s+0.25*N*z4−0.25*z2*z2;

The S term can be normalized and used as a metric as to the quality ofthe center estimate by dividing by the number of pair combinations(N)(N−1)/2 and then taking the square root

$\sqrt{\frac{2 \cdot S}{N \cdot \left( {N - 1} \right)}}.$The normalized value represents the average distance between the planescreated by the pairs of points and the center of the sphere. The squareroot and multiplication by 2 can be avoided for CPU savings in manycases such as if it is used as a threshold. The metric is useful to knowif the data that is being taken matches a circle. The noise of themagnetometer will affect the normalized value, so any threshold usedwill need to reflect that or be large enough for the worst case.Magnetic disturbances will also cause the normalized value to increase.Signals with a magnetic disturbance should not be used because thesesignals will lead to errors in the bias calculation. Magneticdisturbances are hard to predict. So the threshold will allow a metricto gauge whether the data collected was valid or disturbed with amagnetic disturbance. Once a set of data has been declared valid, eachnew sample can be evaluated to see how well it fits with the rest of thedata. To perform this evaluation the previous value of S (before thenormalization) is stored before the new sample and the value of S withthe current sample. Then, divide by the number of points minus one andtake the square root. Keeping the same units as before,

$\sqrt{\frac{S_{N} - S_{N - 1}}{N - 1}}$would be compared to a threshold. Similarly, the square root could beeliminated since the value before normalization is being compared to aconstant. Further optimizations include multiplying the threshold by thedivisor in these equations as multiplication is typically easier thandivision.Simulation

The metric S can be used so that it can be determined if the values haveconverged. It is difficult to know what the sensor has experiencedenough user motion to converge. The functions used to solve a compassbias can be used to simulate in real time to determine how accuratelythe bias can be computed. The compass measurements will be simulated bycomputing orientation with gyroscopes and/or accelerometers. A constantfield with noise added and a bias added will be converted to a compassmeasurement. When some or all of the constraints used to judge aconvergence are noticed, it will be noticed if the actual measurementshave appeared to converge. If convergence has not taken place then it isassumed that the data was corrupted with a magnetic disturbance, and thedata will be thrown away and the process started over.

Data Range

While recording samples of magnetometer data, the minimum and maximum ofeach axis could be stored along with its companion axes values. Then itcan be determined that minimum and maximum values are far enough apartbut not too far apart before calling the data converged. The differenceof the max and min should not exceed twice the radius plus error terms.If the difference is too large, the difference can be called a magneticdisturbance. The data can be thrown way and the process will berestarted. Because the bias can be solved at any point in time, it ispossible to see how large an angle is made using these maximums andminimums. One metric would be to take the dot product going from thecenter of the sphere to the maximum with the vector going from thecenter of the sphere to the minimum. If the dot product is negative, atleast a 90 degree rotation along the perpendicular axis has beenobserved. The sign of the dot product could be used for all axes, but inpractice two out of three seems to work well enough and give enoughobservability to come up with a reasonable magnetometer bias. A smallerangle than 90 degrees could also be allowed using trigonometry.

Eigenvalue

A metric that works well that tracks convergence is looking at theminimum eigenvalue of the matrix in Equation 8. However, this metric isdifficult to meet and typically not used.

Constant Bias

Another metric is used to determine there is a good estimate of themagnetometer bias is if the magnetometer bias stays within a small rangefor a period of time.

Bias Changing

One problem is to address what to do if the magnetometer bias changes.For example, if the magnetometer was in a phone and someone attached amagnetic faceplate to the phone for decoration, the magnetometer biaswould change. After the data has converged, the parameters are kept fora second set of data. Data is added to the solved for set, if

$\sqrt{\frac{S_{N} - S_{N - 1}}{N - 1}}$is below a threshold (could remove the square root), then the data isadded to the old data set. Data is always added to the new data set.After the new data set meets convergence as the first data set had to gothrough, it will be determined how many points were added to the olddata set. If a small amount of points were added to the new data set andthe bias has moved, the new data set will be used as the set to computethe bias, and the old data thrown. In either case, new magnetometer datawill be collected into a new data set.

To avoid numerical issues with the data set, after a set number of datapoints is determined, all the data is divided by a constant, includingthe number of points. Mixing of data could also be performed in similarto the way a filter mixes data from different time samples.

Distance to Center

Another quality metric could be how far the data point is from thecenter of the circle. The data should be within the computed radius anderrors.

Solving for Ellipsoids

The above equations are written for spheres. If the sensitivity of themagnetometer differs on each axis, the resulting shape would be anellipsoid. If the sensitivity is different on each axis, each axis couldbe scaled by the required constant to make it a sphere. The algorithmrequires the sensitivity to be the same along each of the 3 axes.Instead of trying to solve for each sensitivity adjustment, theadjustment could just assume 1 for one axis and solve for the other two.The computation is simpler if the scale factor is applied to both themagnetometer data and also the bias solved for.

Below is the minimum with scale factors on x and z of sx and sz.

$\begin{matrix}{S = \left\lbrack \begin{bmatrix}{{\left\lbrack {{{sx} \cdot a} - \frac{{sx} \cdot \left( {x_{m} + x_{n}} \right)}{2}} \right\rbrack \cdot \left( {x_{m} - x_{n}} \right) \cdot {sx}} + {\left\lbrack {b - \frac{\left( {y_{m} + y_{n}} \right)}{2}} \right\rbrack \cdot}} \\{\left( {y_{m} - y_{n}} \right) + {\left\lbrack {{{sz} \cdot c} - \frac{{sz} \cdot \left( {z_{m} + z_{n}} \right)}{2}} \right\rbrack \cdot \left( {z_{m} - z_{n}} \right) \cdot {sz}}}\end{bmatrix}^{2} \right\rbrack} & (15)\end{matrix}$

Examples of the derivatives which are set to zero are shown in equations(16):

$\begin{matrix}\left. \begin{matrix}{\frac{\mathbb{d}S}{\mathbb{d}{sx}} = \begin{matrix}{{- 2} \cdot {sx} \cdot \left( {x_{m} - x_{n}} \right) \cdot \left( {x_{m} - {2 \cdot a} + x_{n}} \right) \cdot} \\\begin{bmatrix}{{a \cdot {sx}^{2} \cdot x_{m}} - \frac{{sx}^{2} \cdot \left( x_{m} \right)^{2}}{2} + \frac{{sx}^{2} \cdot \left( x_{n} \right)^{2}}{2} - {a \cdot {sx}^{2} \cdot x_{n}} -} \\{\frac{{sz}^{2} \cdot \left( z_{m} \right)^{2}}{2} + {c \cdot {sz}^{2} \cdot z_{m}} + \frac{{sz}^{2} \cdot \left( z_{n} \right)^{2}}{2} - {c \cdot {sz}^{2} \cdot z_{n}} -} \\{\frac{\left( y_{m} \right)^{2}}{2} + {b \cdot y_{m}} + \frac{\left( y_{n} \right)^{2}}{2} - {b \cdot y_{n}}}\end{bmatrix}\end{matrix}} \\{\frac{\mathbb{d}S}{\mathbb{d}a} = \begin{matrix}{2 \cdot {{sx}^{2}\left( {x_{m} - x_{n}} \right)} \cdot} \\\begin{bmatrix}\begin{matrix}{{a \cdot {sx}^{2} \cdot x_{m}} - \frac{{sx}^{2} \cdot \left( x_{m} \right)^{2}}{2} + \frac{{sx}^{2} \cdot \left( x_{n} \right)^{2}}{2} - {a \cdot {sx}^{2} \cdot x_{n}} -} \\{\frac{{sz}^{2} \cdot \left( z_{m} \right)^{2}}{2} + {c \cdot {sz}^{2} \cdot z_{m}} + \frac{{sz}^{2} \cdot \left( z_{n} \right)^{2}}{2} - {c \cdot {sz}^{2} \cdot z_{n}} -}\end{matrix} \\{\frac{\left( y_{m} \right)^{2}}{2} + {b \cdot y_{m}} + \frac{\left( y_{n} \right)^{2}}{2} - {b \cdot y_{n}}}\end{bmatrix}\end{matrix}}\end{matrix} \right\} & (16)\end{matrix}$

The derivates are computed for b, c, and sz as was the case for equation16 above. Then, equation 16 will be set to zero and be reduced as withthe sphere case. The square of the scale factors sx² and sz² will besolved for in equation 16.

Although the present invention has been described in accordance with theembodiments shown, one of ordinary skill in the art will readilyrecognize that there could be variations to the embodiments and thosevariations would be within the spirit and scope of the presentinvention. Accordingly, many modifications may be made by one ofordinary skill in the art without departing from the spirit and scope ofthe appended claims.

What is claimed is:
 1. A computer implemented method of estimating amagnetometer bias for a device comprising: collecting magnetometer datafrom the device; and calculating using a computer a center of a shape ofthe magnetometer data as a result of minimization; wherein themagnetometer data is represented by data points, wherein theminimization is a derivative of a sum of weighted distances of pairs ofdata points, wherein each of the weighted distances is weighted basedupon how far apart the pair of points are from each other; wherein theminimization of calculating the center of the shape further comprises:calculating a plurality of running sums of the magnetometer data;storing the plurality of running sums; storing a count of the number ofterms in each of the running sums; and calculating the center of theshape and setting the estimated magnetometer bias to the center of theshape.
 2. The computer implemented method of claim 1, wherein the shapecomprises a sphere.
 3. The computer implemented method of claim 1,wherein the shape comprises an ellipsoid.
 4. The computer implementedmethod of claim 1, wherein the calculating comprises calculating aplurality of running sums of the magnetometer data, a plurality ofrunning sums of the square of the magnetometer data, a plurality ofrunning sums of the cube of the magnetometer data and a plurality ofrunning sums of the cross products of the magnetometer data.
 5. Thecomputer implemented method of claim 1, which includes evaluating if theestimated bias is accurate.
 6. The computer implemented method of claim1, further comprises calculating a radius of the sphere utilizing theplurality of running sums.
 7. The computer implemented method of claim6, wherein the calculated plurality of running sums are normalizedutilizing the minimization to establish a threshold.
 8. The computerimplemented method of claim 7, wherein the threshold can be utilized todetermine if the estimated magnetometer bias has converged.
 9. Thecomputer implemented method of claim 6, wherein it is determined if aparticular magnetometer data point is on the sphere based upon thecomputed minimization.
 10. The computer implemented method of claim 1,wherein a data range can be utilized to track convergence of themagnetometer bias.
 11. The computer implemented method of claim 1,wherein a minimum eigenvalue of a matrix is utilized to trackconvergence of the bias.
 12. The computer implemented method of claim 1,which includes determining if the magnetometer bias remains within apredetermined range for a predetermined time period to track convergenceof the bias.
 13. The computer implemented method of claim 1, whichexcludes magnetometer data being used as part of the magnetometer biascalculation, based upon the absolute value of the average normalizedmagnetometer data being below a threshold.
 14. The computer implementedmethod of claim 1, wherein the sum of the weighted distances comprisesthe sum of the weighted distances squared.
 15. A device comprising: aprocessor; a magnetometer for providing magnetometer data; and a memorycoupled to the processor; wherein the memory receives the magnetometerdata; wherein the memory includes an algorithm for estimating the biasof the magnetometer data; wherein the processor executes the algorithmto perform the following operations; calculating a center of a shape ofthe magnetometer data as a result of minimization; wherein themagnetometer data is represented by data points, wherein theminimization is a derivative of a sum of weighted distances of pairs ofdata points, wherein each of the weighted distances is weighted basedupon how far apart the pairs of data points are from each other; whereinthe minimization of calculating the center of the shape furthercomprises calculating a plurality of running sums of the magnetometerdata; storing the plurality of running sums; storing a count of thenumber of terms in each of the running sums; and calculating the centerof the shape and setting the estimated magnetometer bias to the centerof the shape.
 16. The device of claim 15, wherein the shape comprises asphere.
 17. The device of claim 16, wherein it is determined if aparticular magnetometer data point is on the sphere based upon thecomputed minimization.
 18. The device of claim 15, wherein the shapecomprises an ellipsoid.
 19. The device of claim 15, wherein calculatinga plurality of running sums of the magnetometer data, a plurality ofrunning sums of the square of the magnetometer data, a plurality ofrunning sums of the cube of the magnetometer data and a plurality ofrunning sums of the cross products of the magnetometer data.
 20. Thedevice of claim 15, wherein the minimization is computed utilizing theplurality of running sums.
 21. The device of claim 20, further comprisescalculating a radius of the sphere utilizing the plurality of runningsums.
 22. The device of claim 15, wherein the calculated plurality ofrunning sums are normalized utilizing the minimization to establish athreshold.
 23. The device of claim 22, wherein the threshold can beutilized to determine if the estimated magnetometer bias has converged.24. The device of claim 15, wherein a data range can be utilized totrack convergence of the magnetometer bias.
 25. The device of claim 15,wherein a minimum eigenvalue of a matrix is utilized to trackconvergence of the bias.
 26. The device of claim 15, which includesdetermining if the magnetometer bias remains within a predeterminedrange for a predetermined time period to track convergence of the bias.27. The device of claim 15, wherein the device comprises any of aportable phone, laptop, tablet, notebook, and smartphone.
 28. The deviceof claim 15, wherein the sum of the weighted distances comprises the sumof the weighted distances squared.
 29. A non-transitory computer programproduct including a computer readable medium to be executed on acomputer, the computer readable medium for performing the followingoperations comprising: collecting magnetometer data from the device; andcalculating a center of a shape of the magnetometer data as a result ofminimization; wherein the magnetometer data is represented by datapoints, wherein the minimization is a derivative of a sum of weighteddistances of pairs of data points, wherein each of the weighteddistances is weighted based upon how far apart the pairs of data pointsare from each other; wherein the minimization of calculating of a centerof an the shape further comprises: calculating a plurality of runningsums of the magnetometer data; storing the plurality of running sums;storing a count of the number of terms of running sums; and calculatingthe bias from the stored running sums when the count of the plurality ofrunning sums is less than a predetermined threshold.
 30. The computerimplemented method of claim 29, wherein the calculating comprisescalculating a plurality of running sums of the magnetometer data, aplurality of running sums of the square of the magnetometer data, aplurality of running sums of the cube of the magnetometer data and aplurality of running sums of the cross products of the magnetometerdata.